﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Research.Kinect.Nui;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;

namespace KinectHeadTrack3DDemo
{
    class KinectHeadTracker
    {
   

        private bool isHeadtracking;
        
        private Vector3 headPosition = new Vector3(0, 0, 0);


        public Vector3 HeadPosition
        {
            get { return headPosition; }
        }

        public bool IsHeadtracking
        {
            get { return isHeadtracking; }
        }

        public void StartHeadtracking()
        {
            nuiInitialization();
            headPosition.Z = 1.5f;
        }


        public void StopHeadtracking()
        {
            isHeadtracking = false;
        }

        public delegate void HeadPositionChangedEventHandler(object sender, Vector3 headPosition);
        public event HeadPositionChangedEventHandler HeadPositionChanged;

        protected void OnHeadPositionChanged()
        {
            HeadPositionChangedEventHandler handler = HeadPositionChanged;
            if (handler != null)
            {
                handler(this, headPosition);
            }
        }

        Runtime nui = new Runtime();

        double minDistance = 1.0;
        double maxDistance = 2.2;

        private void nuiInitialization()
        {
            try
            {
                nui.Initialize(RuntimeOptions.UseSkeletalTracking);

                nui.SkeletonFrameReady += new EventHandler<SkeletonFrameReadyEventArgs>(nui_SkeletonFrameReady);

                nui.SkeletonEngine.TransformSmooth = true;

                var parameters = new TransformSmoothParameters
                {
                    Smoothing = 0.75f, //0.75f
                    Correction = 0.5f, //0.0f
                    Prediction = 0.0f, //0.0f
                    JitterRadius = 0.50f, //0.05f
                    MaxDeviationRadius = 0.05f //0.04f
                };

                nui.SkeletonEngine.SmoothParameters = parameters;                
            }
            catch (InvalidOperationException ex)
            {
                //failed to init kinect
                throw ex;
            }
        }

        void nui_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {

            foreach (var skeleton in e.SkeletonFrame.Skeletons)
            {
                if (skeleton.TrackingState != SkeletonTrackingState.Tracked)
                {
                    isHeadtracking = false;
                    continue;
                }


                foreach (Joint joint in skeleton.Joints)
                {
                    if (joint.Position.W < 0.8f || joint.TrackingState != JointTrackingState.Tracked)
                    {
                        isHeadtracking = false;
                        continue;
                    }

                    if (joint.ID == JointID.Head)
                    {

                        if (joint.Position.Z >= minDistance && joint.Position.Z <= maxDistance)
                        {
                            headPosition.X = joint.Position.X;
                            headPosition.Y = joint.Position.Y * (-1);
                            headPosition.Z = joint.Position.Z;

                            isHeadtracking = true;
                            OnHeadPositionChanged();
                        }
                    }
                }
            }
        }        
    }
}
